
import time
from ArmIK.ArmMoveIK import *
import Board

if __name__ == "__main__":
    AK = ArmIK()
  
  
def move_arm_to_position(x, y, z):  
    # 假设AK.setPitchRangeMoving是用来移动机械臂的函数  
    # 注意：这里的参数可能需要根据你实际的库或模块进行调整  
    AK.setPitchRangeMoving((x, y, z), 0, -150, 0, 2500)  # 假设这是正确的调用方式  
    time.sleep(1)  # 等待机械臂移动到新位置  
  
def main():  
    print("请输入机械臂的坐标 (x, y, z)，用空格分隔，输入'q'退出：")  
    while True:  
        input_str = input()  # 获取用户输入的字符串  
        if input_str.lower() == 'q':  # 如果用户输入'q'，则退出循环  
            break  
        try:  
            x, y, z = map(int, input_str.split())  # 将字符串转换为浮点数  
            move_arm_to_position(x, y, z)  # 调用函数移动机械臂  
        except ValueError:  
            print("输入的坐标格式不正确，请输入数字并用空格分隔。")  
  
if __name__ == "__main__":  
    main()



    #setBusServoPulse(1, 817, 1000)
    #time.sleep(1)
    #setBusServoPulse(1, 217, 1000)
    #time.sleep(1)

    #AK.setPitchRangeMoving((0,0,0),0,-90,0,1000)
    #time.sleep(1)
    #print(f"{AK.setPitchRangeMoving((-15,20,20),0,-90,0,2000)}")
    #time.sleep(1)
    #AK.setPitchRangeMoving((-30,40,20),0,-150,0,1000)
    #time.sleep(1)
    # setBusServoPulse(2, 499, 1000)
    # time.sleep(1)
    # setBusServoPulse(3, 382, 1000)
    # time.sleep(1)
    # setBusServoPulse(4, 869, 1000)
    # time.sleep(1)
    # setBusServoPulse(5, 395, 1000)
    # time.sleep(1)
    # setBusServoPulse(6,357, 1000)
    # time.sleep(1)


    # Board.unloadBusServo(1, 0)

    print('\n结束\n')







